#include "AnimNui.h"

#include <mmsystem.h>
#include <assert.h>
#include <strsafe.h>

//-------------------------------------------------------------------
// Nui_Zero
//
// Zero out member variables
//-------------------------------------------------------------------
void AnimNui::Nui_Zero()
{
	if (m_pNuiSensor)
	{
		m_pNuiSensor->Release();
		m_pNuiSensor = NULL;
	}
	m_hNextDepthFrameEvent = NULL;
	m_hNextColorFrameEvent = NULL;
	m_hNextSkeletonEvent = NULL;
	m_pDepthStreamHandle = NULL;
	m_pVideoStreamHandle = NULL;
	m_hThNuiProcess = NULL;
	m_hEvNuiProcessStop = NULL;
	m_LastSkeletonFoundTime = 0;
	ZeroMemory(m_SkeletonIds,sizeof(m_SkeletonIds));
	ZeroMemory(m_TrackedSkeletonIds,sizeof(m_SkeletonIds));
	tick = 0;

	for (int i = 0; i < NUI_SKELETON_COUNT; i++)
	{
		currentSkeleton[i] = NULL;
	}
}

//-------------------------------------------------------------------
// Nui_Init
//
// Initialize Kinect
//-------------------------------------------------------------------
HRESULT AnimNui::Initialize()
{
	HRESULT  hr;

	if (!m_pNuiSensor)
	{
		HRESULT hr = NuiCreateSensorByIndex(0, &m_pNuiSensor);

		if (FAILED(hr))
		{
			return hr;
		}

		SysFreeString(m_instanceId);

		m_instanceId = m_pNuiSensor->NuiDeviceConnectionId();
	}

	m_hNextDepthFrameEvent = CreateEvent( NULL, TRUE, FALSE, NULL );
	m_hNextColorFrameEvent = CreateEvent( NULL, TRUE, FALSE, NULL );
	m_hNextSkeletonEvent = CreateEvent( NULL, TRUE, FALSE, NULL );

	DWORD nuiFlags = NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX | NUI_INITIALIZE_FLAG_USES_SKELETON |  NUI_INITIALIZE_FLAG_USES_COLOR;
	hr = m_pNuiSensor->NuiInitialize( nuiFlags );
	
	if (E_NUI_SKELETAL_ENGINE_BUSY == hr)
	{
		nuiFlags = NUI_INITIALIZE_FLAG_USES_DEPTH |  NUI_INITIALIZE_FLAG_USES_COLOR;
		hr = m_pNuiSensor->NuiInitialize( nuiFlags) ;
	}

	if (FAILED(hr))
	{
		return hr;
	}

	if ( HasSkeletalEngine( m_pNuiSensor))
	{
		hr = m_pNuiSensor->NuiSkeletonTrackingEnable( m_hNextSkeletonEvent, 0 );
		
		if( FAILED( hr ) )
		{
			return hr;
		}
	}

	hr = m_pNuiSensor->NuiImageStreamOpen(
		NUI_IMAGE_TYPE_COLOR,
		NUI_IMAGE_RESOLUTION_640x480,
		0,
		2,
		m_hNextColorFrameEvent,
		&m_pVideoStreamHandle );

	if (FAILED(hr))
	{
		return hr;
	}

	hr = m_pNuiSensor->NuiImageStreamOpen(
		HasSkeletalEngine(m_pNuiSensor) ? NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX : NUI_IMAGE_TYPE_DEPTH,
		NUI_IMAGE_RESOLUTION_320x240,
		0,
		2,
		m_hNextDepthFrameEvent,
		&m_pDepthStreamHandle );

	if (FAILED( hr))
	{
		return hr;
	}

	// Start the Nui processing thread
	m_hEvNuiProcessStop = CreateEvent( NULL, FALSE, FALSE, NULL );
	m_hThNuiProcess = CreateThread( NULL, 0, Nui_ProcessThread, this, 0, NULL );

	return hr;
}

//-------------------------------------------------------------------
// Nui_UnInit
//
// Uninitialize Kinect
//-------------------------------------------------------------------
void AnimNui::DeInitialize()
{	
	// Stop the Nui processing thread
	if ( NULL != m_hEvNuiProcessStop )
	{
		// Signal the thread
		SetEvent(m_hEvNuiProcessStop);

		// Wait for thread to stop
		if ( NULL != m_hThNuiProcess )
		{
			WaitForSingleObject( m_hThNuiProcess, INFINITE );
			CloseHandle( m_hThNuiProcess );		
		}

			CloseHandle( m_hEvNuiProcessStop );
	}

	if ( m_pNuiSensor )
	{
		m_pNuiSensor->NuiShutdown( );
	}
	if ( m_hNextSkeletonEvent && ( m_hNextSkeletonEvent != INVALID_HANDLE_VALUE ) )
	{
		CloseHandle( m_hNextSkeletonEvent );
		m_hNextSkeletonEvent = NULL;
	}
	if ( m_hNextDepthFrameEvent && ( m_hNextDepthFrameEvent != INVALID_HANDLE_VALUE ) )
	{
		CloseHandle( m_hNextDepthFrameEvent );
		m_hNextDepthFrameEvent = NULL;
	}
	if ( m_hNextColorFrameEvent && ( m_hNextColorFrameEvent != INVALID_HANDLE_VALUE ) )
	{
		CloseHandle( m_hNextColorFrameEvent );
		m_hNextColorFrameEvent = NULL;
	}

	if ( m_pNuiSensor )
	{
		m_pNuiSensor->Release();
		m_pNuiSensor = NULL;
	}
}

DWORD WINAPI AnimNui::Nui_ProcessThread(LPVOID pParam)
{
	AnimNui *pthis = (AnimNui *) pParam;
	return pthis->Nui_ProcessThread();
}

//-------------------------------------------------------------------
// Nui_ProcessThread
//
// Thread to handle Kinect processing
//-------------------------------------------------------------------
DWORD WINAPI AnimNui::Nui_ProcessThread()
{
	const int numEvents = 4;
	HANDLE hEvents[numEvents] = { m_hEvNuiProcessStop, m_hNextDepthFrameEvent, m_hNextColorFrameEvent, m_hNextSkeletonEvent };
	int    nEventIdx;

	//blank the skeleton display on startup
	m_LastSkeletonFoundTime = 0;

	// Main thread loop
	
	while(tick < 3)
	{
		// Wait for any of the events to be signalled
		nEventIdx = WaitForMultipleObjects( numEvents, hEvents, FALSE, 100 );

		// Process signal events
		switch ( nEventIdx )
		{
		case WAIT_TIMEOUT:
			continue;

			// If the stop event, stop looping and exit
		case WAIT_OBJECT_0:
			tick = 0;
			continue;

		case WAIT_OBJECT_0 + 1:
			Nui_GotDepthAlert();
			tick++;
			break;

		case WAIT_OBJECT_0 + 2:
			Nui_GotColorAlert();
			tick++;
			break;

		case WAIT_OBJECT_0 + 3:
			Nui_GotSkeletonAlert( );
			tick++;
			break;
		}		
	}

	tick = 0;
	return 0;
}

void AnimNui::Nui_GotColorAlert( )
{
	NUI_IMAGE_FRAME imageFrame;

	HRESULT hr = m_pNuiSensor->NuiImageStreamGetNextFrame( m_pVideoStreamHandle, 0, &imageFrame );
	m_pNuiSensor->NuiImageStreamReleaseFrame( m_pVideoStreamHandle, &imageFrame );
}

//-------------------------------------------------------------------
// Nui_GotDepthAlert
//
// Handle new depth data
//-------------------------------------------------------------------
void AnimNui::Nui_GotDepthAlert( )
{
	NUI_IMAGE_FRAME imageFrame;

	HRESULT hr = m_pNuiSensor->NuiImageStreamGetNextFrame(m_pDepthStreamHandle,0,&imageFrame );
	m_pNuiSensor->NuiImageStreamReleaseFrame( m_pDepthStreamHandle, &imageFrame );
}

void AnimNui::Nui_GotSkeletonAlert( )
{
	NUI_SKELETON_FRAME SkeletonFrame = {0};

	bool bFoundSkeleton = false;

	if ( SUCCEEDED(m_pNuiSensor->NuiSkeletonGetNextFrame( 0, &SkeletonFrame )) )
	{
		for ( int i = 0 ; i < NUI_SKELETON_COUNT ; i++ )
		{
			if( SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_TRACKED ||
				(SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_POSITION_ONLY))
			{
				bFoundSkeleton = true;
			}
		}
	}

	// no skeletons!
	if( !bFoundSkeleton )
	{
		return;
	}

	// smooth out the skeleton data
	HRESULT hr = m_pNuiSensor->NuiTransformSmooth(&SkeletonFrame,NULL);
	if ( FAILED(hr) )
	{
		return;
	}

	bool bSkeletonIdsChanged = false;
	for ( int i = 0 ; i < NUI_SKELETON_COUNT; i++ )
	{
		if ( m_SkeletonIds[i] != SkeletonFrame.SkeletonData[i].dwTrackingID )
		{
			m_SkeletonIds[i] = SkeletonFrame.SkeletonData[i].dwTrackingID;
			bSkeletonIdsChanged = true;
		}

		// Show skeleton only if it is tracked, and the center-shoulder joint is at least inferred.
		if ( SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_TRACKED &&
			SkeletonFrame.SkeletonData[i].eSkeletonPositionTrackingState[NUI_SKELETON_POSITION_SHOULDER_CENTER] != NUI_SKELETON_POSITION_NOT_TRACKED)
		{
			currentSkeleton[i] = &SkeletonFrame.SkeletonData[i];
		}
		else if (SkeletonFrame.SkeletonData[i].eTrackingState == NUI_SKELETON_POSITION_ONLY )
		{
			currentSkeleton[i] = &SkeletonFrame.SkeletonData[i];
		}
	}
}

AnimNui::AnimNui( void )
{

}

AnimNui::~AnimNui( void )
{
	DeInitialize();
}

